#include <iostream>
#include <array>
#include <cmath>
#include <sys/socket.h>
#include "ADS_B.h"
#include <cstdio>
//初始化arr容器;

void initarr(std::array<AircraftState*,1024>& otherShips){
    for(auto i:otherShips){
        i= nullptr;
        static int cnt=0;
        std::cout<<cnt++<<std::endl;
    }
}
// 预测飞机未来位置
AircraftState predictFuturePosition(const AircraftState& current, double time) {
    AircraftState future = AircraftState( 0, 0, 0, 0, 0);
    // 简化预测，假设速度恒定且方向不变
    future.x = current.x + current.vx * time;
    future.y = current.y + current.vy * time; // 实际上vy可能接近0，因为经度方向的速度由航向和速度决定
    // 航向在此预测中不直接使用，因为我们假设速度分量是直接给出的
    future.vx=current.vx;
    future.vy=current.vy;
    future.heading=current.heading;
    return future;
}

// 计算两点之间的欧几里得距离
double calculateEuclideanDistance(const AircraftState& a, const AircraftState& b) {
    return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2) );
}

// 检查是否存在碰撞风险
bool checkCollisionRisk(int id,AircraftState &ownShip, std::array<AircraftState*,1024>& otherShips, double minSafeDistance, double lookAheadTime) {
    for(int i=0;i<otherShips.size();i++){
        // 预测未来位置
        if(*otherShips[i]!=ownShip&& (otherShips[i] != nullptr)) {
            AircraftState predictedOwnShip = predictFuturePosition(ownShip, lookAheadTime);
            AircraftState predictedOtherShip = predictFuturePosition(*otherShips[i], lookAheadTime);

            // 计算预测位置之间的距离
            double distance = calculateEuclideanDistance(predictedOwnShip, predictedOtherShip);

            // 如果距离小于最小安全距离，则认为存在碰撞风险
            if (distance < minSafeDistance) {//防撞规避
                    if (predictedOwnShip.x > 0 && predictedOwnShip.y < 0 &&
                         (predictedOwnShip.heading) > 270 && predictedOwnShip.heading < 360) {
                        if (predictedOtherShip.x > 0 && predictedOtherShip.y < 0 &&//第四象限
                            (predictedOtherShip.heading> 270 && (predictedOtherShip.heading) < 360)) {
                            ownShip.heading = ((int) (ownShip.heading + 45) % 360);
                            otherShips[i]->heading =((int) (otherShips[i]->heading - 45) % 360);
                        }//第一象限
                        if(predictedOtherShip.x > 0 && predictedOtherShip.y < 0 &&predictedOtherShip.heading > 180&&predictedOtherShip.heading<270){
                            otherShips[i]->heading =((int) (otherShips[i]->heading - 45) % 360);
                            ownShip.heading = ((int) ( ownShip.heading - 45) % 360);
                        }//第二象限
                        if(predictedOtherShip.x<0&&predictedOtherShip.y>0&&predictedOtherShip.heading>90&&predictedOtherShip.heading<180){
                            otherShips[i]->heading=otherShips[i]->heading+45;
                            ownShip.heading=((int)( ownShip.heading+45)%360);
                        }
                        if(predictedOtherShip.x<0&&predictedOtherShip.y<0&&predictedOtherShip.heading>0&&predictedOtherShip.heading<90){
                           otherShips[i]->heading =(360-std::abs(((int)(otherShips[i]->heading-45)%360)));
                           ownShip.heading=((int)( ownShip.heading+45)%360);
                        }
                    }


                std::cout<<"bust"<<std::endl;
                char bust[50];//发送改变的数据给本机
                char vx[10],vy[10],x[10],y[10],heading[10];
                memset(bust,0,sizeof(bust));
                memset(vx,0,sizeof(vx));
                memset(vy,0,sizeof(vy));
                memset(x,0,sizeof(x));
                memset(y,0,sizeof(y));
                memset(heading,0,sizeof(heading));
                std::sprintf(x,"%.2f",ownShip.x);
                std::sprintf(y,"%.2f",ownShip.y);
                std::sprintf(vx,"%.2f",ownShip.vx);
                std::sprintf(vy,"%.2f",ownShip.vy);
                std::sprintf(heading,"%.2f",ownShip.heading);
                strcpy(bust, x);
                strcpy(bust+10, y);
                strcpy(bust+20, vx);
                strcpy(bust+30, vy);
                strcpy(bust+40, heading);
                printf("own send x=%s,y=%s",std::to_string(ownShip.x).c_str(),std::to_string(ownShip.y).c_str());
                printf("xv=%s,yv=%s,hx=%s\n",std::to_string(ownShip.vx).c_str(),std::to_string(ownShip.vy).c_str(),std::to_string(ownShip.heading).c_str());
                int sen=send(id,bust,50,0);
                if(sen==-1){
                    std::cout<<"own send error"<<std::endl;
                }sen=0;
                memset(bust,0,sizeof(bust));
                memset(vx,0,sizeof(vx));
                memset(vy,0,sizeof(vy));
                memset(x,0,sizeof(x));
                memset(y,0,sizeof(y));
                memset(heading,0,sizeof(heading));
                std::sprintf(x,"%.2f",otherShips[i]->x);
                std::sprintf(y,"%.2f",otherShips[i]->y);
                std::sprintf(vx,"%.2f",otherShips[i]->vx);
                std::sprintf(vy,"%.2f",otherShips[i]->vy);
                std::sprintf(heading,"%.2f",otherShips[i]->heading);
                strcpy(bust, x);
                strcpy(bust+10, y);
                strcpy(bust+20, vx);
                strcpy(bust+30, vy);
                strcpy(bust+40, heading);
                printf("other send x=%s,y=%s",std::to_string(otherShips[i]->x).c_str(),std::to_string(otherShips[i]->y).c_str());
                printf("xv=%s,yv=%s,hx=%s\n",std::to_string(otherShips[i]->vx).c_str(),std::to_string(otherShips[i]->vy).c_str(),std::to_string(otherShips[i]->heading).c_str());
                sen=send(i,bust,50,0);
                if(sen==-1){
                    std::cout<<"other send error"<<std::endl;
                }
                return true;//存在风险

            }
            }
        }
    return false;
}
